#include "ngio.h"
#include <stdio.h>
#include <string.h>
#include <windows.h>
#include <QtCore>
#pragma warning(disable: 4996)

#include "dynamometers/NGIO_lib_interface.h"

#define MAX_NUM_MEASUREMENTS 200

static void OSSleep(unsigned int msToSleep);

NGIO_LIBRARY_HANDLE g_hNGIOlib = NULL;
NGIO_DEVICE_HANDLE g_hDevice = NULL;

char tmpstring[300];
gtype_uint16 majorVersion, minorVersion;
gtype_int32 status = 0;
gtype_uint32 sig, mask, deviceType;
gtype_uint32 numDevices;
NGIO_DEVICE_LIST_HANDLE hDeviceList;
char deviceName[NGIO_MAX_SIZE_DEVICE_NAME];
NGIOGetStatusCmdResponsePayload getStatusResponse;
NGIO_NVMEM_CHANNEL_ID1_rec getNVMemResponse;
gtype_uint32 nRespBytes;
signed char channel;
char units[20];

gtype_int32 rawMeasurements[MAX_NUM_MEASUREMENTS];
gtype_real32 volts[MAX_NUM_MEASUREMENTS];
gtype_real32 calbMeasurements[MAX_NUM_MEASUREMENTS];
gtype_int32 numMeasurements, i;
gtype_real32 averageCalbMeasurement;
float leftav = 0;
float rightav = 0;
unsigned char sen;


/**********************
 * Returns newest reading for 'left' dynamometer (analog-2)
 */
float Ngio::getLeft()
{
    return leftav;
}
// end getLeft()


/**********************
 * Returns newest reading for 'right' dynamometer (analog-3)
 */
float Ngio::getRight()
{
    return rightav;
}
// end getRight()


/**********************
 * Returns success of communicating with the device
 */
bool Ngio::getStatus(){
    if(g_hDevice) return true;
    else return false;
}
// end getStatus()


/**********************
 * Constructor, loading the drivers and device
 */
Ngio::Ngio()
{ 
    g_hNGIOlib = NGIO_Init();
    NGIO_GetDLLVersion(g_hNGIOlib, &majorVersion, &minorVersion);
}
// end Ngio()


/**********************
 * Enables any available devices, and then initalises communication with
 * any available channels.
 */
void Ngio::FirstStep()
{

    Sleep(1000);//Give Jungo device driver time to find the LabQuest.

    if (g_hNGIOlib)
    {
        deviceType = NGIO_DEVTYPE_LABQUEST;
        NGIO_SearchForDevices(g_hNGIOlib, deviceType, NGIO_COMM_TRANSPORT_USB, NULL, &sig);

        hDeviceList = NGIO_OpenDeviceListSnapshot(g_hNGIOlib, deviceType, &numDevices, &sig);
        status = NGIO_DeviceListSnapshot_GetNthEntry(hDeviceList, 0, deviceName, sizeof(deviceName), &mask);
        NGIO_CloseDeviceListSnapshot(hDeviceList);

        if (0 != status)
        {
            //Couldn't find a LabQuest, so look for a LabQuest Mini.
            deviceType = NGIO_DEVTYPE_LABQUEST_MINI;
            NGIO_SearchForDevices(g_hNGIOlib, deviceType, NGIO_COMM_TRANSPORT_USB, NULL, &sig);

            hDeviceList = NGIO_OpenDeviceListSnapshot(g_hNGIOlib, deviceType, &numDevices, &sig);
            status = NGIO_DeviceListSnapshot_GetNthEntry(hDeviceList, 0, deviceName, sizeof(deviceName), &mask);
            NGIO_CloseDeviceListSnapshot(hDeviceList);

        }

        if (0 != status)
        {
            //   printf("NGIO_DeviceCheck cannot find a LabQuest or a LabQuest Mini.\n");
        }
        else
        {
            char deviceDesc[50];
            if (NGIO_DEVTYPE_LABQUEST == deviceType)
                strcpy(deviceDesc, "LabQuest");
            else
                strcpy(deviceDesc, "LabQuest Mini");
            g_hDevice = NGIO_Device_Open(g_hNGIOlib, deviceName, 0);

            if (g_hDevice)
            {
                if (NGIO_DEVTYPE_LABQUEST == deviceType)
                {
                    //Wrest control of the LabQuest data acquisition subsystem(the DAQ) away from the GUI app running
                    //down on the LabQuest.
                    status = NGIO_Device_AcquireExclusiveOwnership(g_hDevice, NGIO_GRAB_DAQ_TIMEOUT);

                }

                if (0 == status)
                {
                    memset(&getStatusResponse, 0, sizeof(getStatusResponse));
                    nRespBytes = sizeof(getStatusResponse);
                    status = NGIO_Device_SendCmdAndGetResponse(g_hDevice, NGIO_CMD_ID_GET_STATUS, NULL, 0, &getStatusResponse,
                                                               &nRespBytes, NGIO_TIMEOUT_MS_DEFAULT);
                }

                if (0 == status)
                {
                    memset(&getNVMemResponse, 0, sizeof(getNVMemResponse));
                    status = NGIO_Device_NVMemBlk_Read(g_hDevice, NGIO_NVMEM_CHANNEL_ID1, &getNVMemResponse, 0,
                                                       sizeof(getNVMemResponse) - 1, NGIO_TIMEOUT_MS_DEFAULT);
                }

                if (0 == status)
                {
                    unsigned int serialNum = getNVMemResponse.serialNumber.msbyteMswordSerialCounter;
                    serialNum = (serialNum << 8) + getNVMemResponse.serialNumber.lsbyteMswordSerialCounter;
                    serialNum = (serialNum << 8) + getNVMemResponse.serialNumber.msbyteLswordSerialCounter;
                    serialNum = (serialNum << 8) + getNVMemResponse.serialNumber.lsbyteLswordSerialCounter;
                }

                if (0 == status)
                {
                    NGIOSetSensorChannelEnableMaskParams maskParams;
                    NGIOSetAnalogInputParams analogInputParams;
                    unsigned char sensorId = 0;
                    unsigned char channelMask = NGIO_CHANNEL_MASK_ANALOG1;
                    gtype_uint32 sig;
                    memset(&maskParams, 0, sizeof(maskParams));
                    for (channel = NGIO_CHANNEL_ID_ANALOG1; channel <= NGIO_CHANNEL_ID_ANALOG4; channel++)
                    {
                        NGIO_Device_DDSMem_GetSensorNumber(g_hDevice, channel, &sensorId, 1, &sig, NGIO_TIMEOUT_MS_DEFAULT);
                        if (sensorId != 0)
                        {
                            maskParams.lsbyteLsword_EnableSensorChannels = maskParams.lsbyteLsword_EnableSensorChannels | channelMask;
                            if (sensorId >= kSensorIdNumber_FirstSmartSensor)
                                NGIO_Device_DDSMem_ReadRecord(g_hDevice, channel, 0, NGIO_TIMEOUT_MS_READ_DDSMEMBLOCK);

                            if (kProbeTypeAnalog10V == NGIO_Device_GetProbeType(g_hDevice, channel))
                                analogInputParams.analogInput = NGIO_ANALOG_INPUT_PM10V_BUILTIN_12BIT_ADC;
                            else
                                analogInputParams.analogInput = NGIO_ANALOG_INPUT_5V_BUILTIN_12BIT_ADC;
                            analogInputParams.channel = channel;
                            NGIO_Device_SendCmdAndGetResponse(g_hDevice, NGIO_CMD_ID_SET_ANALOG_INPUT, &analogInputParams,
                                                              sizeof(analogInputParams), NULL, NULL, NGIO_TIMEOUT_MS_DEFAULT);
                        }
                        channelMask = channelMask << 1;
                    }

                    if (0 == maskParams.lsbyteLsword_EnableSensorChannels){}
                    else
                    {
                        NGIO_Device_SendCmdAndGetResponse(g_hDevice, NGIO_CMD_ID_SET_SENSOR_CHANNEL_ENABLE_MASK, &maskParams,
                                                          sizeof(maskParams), NULL, NULL, NGIO_TIMEOUT_MS_DEFAULT);

                        NGIO_Device_SetMeasurementPeriod(g_hDevice, -1, 0.010, NGIO_TIMEOUT_MS_DEFAULT);//10 milliseconds => 100 hz.

                        NGIO_Device_SendCmdAndGetResponse(g_hDevice, NGIO_CMD_ID_START_MEASUREMENTS, NULL, 0, NULL, NULL, NGIO_TIMEOUT_MS_DEFAULT);

                        Sleep(1000);
                        sen = sensorId;

                    }
                }
            }
        }
    }
}


/**********************
 * Handles closure and disconnection of the device
 */
void Ngio::ThirdStep(){
    NGIO_Device_Close(g_hDevice);
    g_hDevice = NULL;
    NGIO_Uninit(g_hNGIOlib);
    g_hNGIOlib = NULL;
}
// end ThirdStep(){


/**********************
 * Reads measurements from each device for time frame, and averages them
 */
void Ngio::SecondStep()
{
    unsigned char sensorId = sen;

    //for each analog channel - 2 = 'left', 3 = 'right'
    for (channel = NGIO_CHANNEL_ID_ANALOG2; channel <= NGIO_CHANNEL_ID_ANALOG3; channel++)
    {
        NGIO_Device_DDSMem_GetSensorNumber(g_hDevice, channel, &sensorId, 0, &sig, 0);
        if (sensorId != 0)
        {
            char longname[30];
            longname[0] = 0;
            NGIO_Device_DDSMem_GetLongName(g_hDevice, channel, longname, sizeof(longname));

            int probeType = NGIO_Device_GetProbeType(g_hDevice, channel);
            numMeasurements = NGIO_Device_ReadRawMeasurements(g_hDevice, channel, rawMeasurements, NULL, MAX_NUM_MEASUREMENTS);
            if (numMeasurements > 0)
            {
                averageCalbMeasurement = 0.0;
                for (i = 0; i < numMeasurements; i++)
                {
                    volts[i] = NGIO_Device_ConvertToVoltage(g_hDevice, channel, rawMeasurements[i], probeType);
                    calbMeasurements[i] = NGIO_Device_CalibrateData(g_hDevice, channel, volts[i]);
                    averageCalbMeasurement += calbMeasurements[i];
                }
                if (numMeasurements > 1)
                    averageCalbMeasurement = averageCalbMeasurement/numMeasurements;

                gtype_real32 a, b, c;
                unsigned char activeCalPage = 0;
                NGIO_Device_DDSMem_GetActiveCalPage(g_hDevice, channel, &activeCalPage);
                NGIO_Device_DDSMem_GetCalPage(g_hDevice, channel, activeCalPage, &a, &b, &c, units, sizeof(units));
                if(channel == NGIO_CHANNEL_ID_ANALOG2) leftav = averageCalbMeasurement;  // set left dynamometer average (analog2)
                else rightav = averageCalbMeasurement; // set right dynamometer average (analog3)
            }

        }
    }

}
// end SecondStep()
